#include "sbot_protocol.h"

sp_element_size_t _sbot_protocol_element_size_is(sp_element_id_t element_type)
{
    return sp_element_type[element_type - 1];
}

int _sbot_protocol_calculate_message_size_is()
{
    int               icnt         = 1;
    sp_message_size_t size_message = 0;

    /* Calculate message size */
    for(; icnt < SF_COUNT; icnt++)
    {
        size_message += _sbot_protocol_element_size_is(icnt);
    }

    return size_message;
}

sp_element_map_t* _sbot_protocol_element_indexes_map_create(void)
{
    int               icnt                = 1;
    sp_message_size_t size_message        = 0;
    sp_element_map_t* element_indexes_map = NULL;

    element_indexes_map = (sp_element_map_t*)malloc(sizeof(sp_element_map_t) * SF_COUNT);

    for(; icnt < SF_COUNT; icnt++)
    {
        element_indexes_map[icnt - 1] = size_message;
        size_message += _sbot_protocol_element_size_is(icnt);
    }

    return element_indexes_map;
}

sp_packet_t sbot_protocol_packet_create(void)
{
    sp_packet_t sp_packet = NULL;

    sp_packet               = (sp_packet_t)malloc(sizeof(struct sp_packet));
    sp_packet->size_header  = strlen(sbot_protocol_message_header);
    sp_packet->size_message = sp_packet->size_header;                      /* The message size contains a header size */
    sp_packet->size_message += _sbot_protocol_calculate_message_size_is(); /* Add message size */
    sp_packet->size_message += 1;                                          /* Add crc size */

    sp_packet->p_message = (uint8_t*)malloc((sp_packet->size_message));
    memset(sp_packet->p_message, 0, sp_packet->size_message);      /* Clear message buffer */
    memcpy(sp_packet->p_message, sbot_protocol_message_header, 3); /* Copy message header to message buffer */

    sp_packet->element_indexes_map = _sbot_protocol_element_indexes_map_create();

    return sp_packet;
}

uint8_t crc8(const unsigned char* data, size_t len)
{
    uint8_t crc = 0xFF;
    while(len--)
    {
        crc ^= *data++;
        for(unsigned char i = 8; i; i--)
        {
            crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : (crc << 1);
        }
    }
    return crc;
}

void sbot_protocol_alter(sp_packet_t sp_packet_p, sp_element_id_t element_id, void* value_p)
{
    sp_element_size_t elm_size   = 0;
    sp_element_map_t  elm_indexs = 0;

    elm_size   = _sbot_protocol_element_size_is(element_id);
    elm_indexs = sp_packet_p->element_indexes_map[element_id - 1];
    elm_indexs += strlen(sbot_protocol_message_header);

    memcpy(sp_packet_p->p_message + elm_indexs, value_p, elm_size);
}

void sbot_protocol_get(sp_packet_t sp_packet_p, sp_element_id_t element_id, void* value_p)
{
    sp_element_size_t elm_size   = 0;
    sp_element_map_t  elm_indexs = 0;

    elm_size   = _sbot_protocol_element_size_is(element_id);
    elm_indexs = sp_packet_p->element_indexes_map[element_id - 1];
    elm_indexs += strlen(sbot_protocol_message_header);

    memcpy(value_p, sp_packet_p->p_message + elm_indexs, elm_size);
}

void sbot_protocol_generate_crc(sp_packet_t sp_packet_p)
{
    uint8_t crc = 0;

    crc = crc8(sp_packet_p->p_message, sp_packet_p->size_message - 1);

    memcpy(sp_packet_p->p_message + sp_packet_p->size_message - 1, &crc, 1);
}

uint8_t sbot_protocol_check_crc(sp_packet_t sp_packet_p)
{
    uint8_t crc   = 0;
    uint8_t crc_p = 0;

    crc = crc8(sp_packet_p->p_message, sp_packet_p->size_message - 1);
    memcpy(&crc_p, sp_packet_p->p_message + sp_packet_p->size_message - 1, 1);

    if(crc == crc_p)
    {
        return 1;
    }

    return 0;
}
